#include "obstacle_detector/convex_hull.h"

namespace obstacle_detector
{
__device__ __forceinline__ uint16_t atomicMinUint16_3(uint16_t *address, uint16_t val)
{
	uint16_t ret = *address;
	while(val < ret)
	{
		uint16_t old = ret;
		ret = atomicCAS(address, old, val);
		if(ret == old)
		{
			break;
		}
	}
	return ret;
}

__device__ __forceinline__ uint16_t atomicMaxUint16_3(uint16_t *address, uint16_t val)
{
	uint16_t ret = *address;
	while(val > ret)
	{
		uint16_t old = ret;
		ret = atomicCAS(address, old, val);
		if(ret == old)
		{
			break;
		}
	}
	return ret;
}

__device__ __forceinline__ float dot_product(float *vector1, float *vector2)
{
	return vector1[0]*vector2[0]+vector1[1]*vector2[1];
}

//vector2 到vector1的距离
__device__ __forceinline__ float dis2line_kernel(float *vector1,float *vector2)
{
	float length=sqrtf(powf(vector1[0],2)+powf(vector1[1],2));
	float cross_product=vector1[0]*vector2[1]-vector2[0]*vector1[1];
	float dis2line=cross_product/length;
	return dis2line;
}

__device__ __forceinline__ float fatomicMax2(float *address, float val)
{
	int ret = __float_as_int(*address);
	while(val > __int_as_float(ret))
	{
		int old = ret;
		ret = atomicCAS((int *)address, old, __float_as_int(val));
		if(ret == old)
		{
			break;
		}
	}
	return __int_as_float(ret);
}

__device__ __forceinline__ float fatomicMin2(float *address, float val)
{
	int ret = __float_as_int(*address);
	while(val < __int_as_float(ret))
	{
		int old = ret;
		ret = atomicCAS((int *)address, old, __float_as_int(val));
		if(ret == old)
		{
			break;
		}
	}
	return __int_as_float(ret);
}

__device__ __forceinline__ float transform2global(float *point, float loc_x,
	float loc_y,float loc_yaw)
{
	float global_x=point[0]*cos(-1*loc_yaw)+point[1]*sin(-1*loc_yaw);
	float global_y=-1*point[0]*sin(-1*loc_yaw)+point[1]*cos(-1*loc_yaw);
	global_x+=loc_x;
	global_y+=loc_y;
	point[0]=global_x;
	point[1]=global_y;
}



__global__ void get_leftright_points_y(const int16_t *points, int point_num,
	const uint8_t *points_label,uint16_t *extreme_points)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=(point_num))
	{
		return;
	}
	uint16_t label=points_label[threadid];
	if (label==0) return;

	uint16_t y=points[threadid*4+1]+32768;
	extreme_points[label*33]=1;
	atomicMaxUint16_3(extreme_points+label*33+2,y);
	atomicMinUint16_3(extreme_points+label*33+18,y);
}

__global__ void get_leftright_points_x(const int16_t *points, int point_num,
	const uint8_t *points_label, uint16_t *extreme_points)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=(point_num))
	{
		return;
	}
	uint16_t label=points_label[threadid];
	if (label==0) return;

	if (extreme_points[label*33]==0) return;

	uint16_t y=points[threadid*4+1]+32768;
	if (y==extreme_points[label*33+2])
	{
		extreme_points[label*33+1]=points[threadid*4+0]+32768;
	}
	if (y==extreme_points[label*33+18])
	{
		extreme_points[label*33+17]=points[threadid*4+0]+32768;
	}
}

//根据找到的最左点和最右点把点集分成两部分, 并且记录各自部分到左右两点连成的直线的最远距离
__global__ void split_points_stage1(const int16_t *points, int point_num,
	const uint8_t *points_label, const uint16_t *extreme_points,
	uint16_t *all_dis2line,uint16_t *minmax_dis2line, uint8_t *split_label)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=(point_num))
	{
		return;
	}
	uint16_t label=points_label[threadid];
	if (label==0) return;

	if (extreme_points[label*33]==0) return;
	float x_left=(extreme_points[label*33+1]-32768)*1.0/256;
	float y_left=(extreme_points[label*33+2]-32768)*1.0/256; 
	float x_right=(extreme_points[label*33+17]-32768)*1.0/256;
	float y_right=(extreme_points[label*33+18]-32768)*1.0/256;
	float x=points[threadid*4+0]*1.0/256;
	float y=points[threadid*4+1]*1.0/256;

	float vector1[2],vector2[2];
	vector1[0]=x_right-x_left;
	vector1[1]=y_right-y_left;
	vector2[0]=x-x_left;
	vector2[1]=y-y_left;

	float dis2line=dis2line_kernel(vector1,vector2);

	uint16_t dis2line_uint16=int16_t(dis2line*256)+32768;
	all_dis2line[threadid]=dis2line_uint16;

	if (dis2line<0)
	{
		split_label[threadid]=2;  //位于直线的下部
		atomicMinUint16_3(minmax_dis2line+label*14+0,dis2line_uint16);
	}
	else if (dis2line>0)
	{
		split_label[threadid]=1;  //位于直线的上部
		atomicMaxUint16_3(minmax_dis2line+label*14+1,dis2line_uint16);
	}
}

//根据最远距离找到第4个和第12个极点
__global__ void get_extreme_points_stage1(const int16_t *points, int point_num,
	const uint8_t *points_label, uint16_t *extreme_points,
	uint16_t *all_dis2line,uint16_t *minmax_dis2line)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=(point_num))
	{
		return;
	}
	uint16_t label=points_label[threadid];
	if (label==0) return;

	if (extreme_points[label*33]==0) return;

	uint16_t dis2line=all_dis2line[threadid];


	if (dis2line==minmax_dis2line[label*14+0])
	{
		extreme_points[label*33+25]=points[threadid*4+0]+32768;
		extreme_points[label*33+26]=points[threadid*4+1]+32768;
	}
	else if (dis2line==minmax_dis2line[label*14+1])
	{
		extreme_points[label*33+9]=points[threadid*4+0]+32768;
		extreme_points[label*33+10]=points[threadid*4+1]+32768;
	}

}

//根据找到的极点把点集分成三部分，三角形内，三角形左边，三角形右边，并且记录各自部分到三角形边的最远距离
//ABC是三角形的三个定点 P是任意一点，AP=u*AC+v*AB来表示, 1>=u>0 1>=v>0 u+v<=1表示点P在三角形ABC内
__global__ void split_points_stage2(const int16_t *points, int point_num,
	const uint8_t *points_label, const uint16_t *extreme_points,
	uint16_t *all_dis2line,uint16_t *minmax_dis2line, uint8_t *split_label)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=(point_num))
	{
		return;
	}
	uint16_t label=points_label[threadid];
	if (label==0) return;

	if (extreme_points[label*33]==0) return;

	uint8_t area_label=split_label[threadid];
	if (area_label==0) return;

	float B_x=(extreme_points[label*33+1]-32768)*1.0/256; //最左端的点
	float B_y=(extreme_points[label*33+2]-32768)*1.0/256;
	float C_x=(extreme_points[label*33+17]-32768)*1.0/256; //最右端的点
	float C_y=(extreme_points[label*33+18]-32768)*1.0/256;

	int A_index=area_label==1 ? 4 : 12; //4是上三角 12是下三角
	// printf ("Aindex %d %d: %d,%d,%d\n",label,area_label,A_index,
	// 	extreme_points[label*33+A_index*2+1],extreme_points[label*33+A_index*2+2]);

	float A_x=(extreme_points[label*33+A_index*2+1]-32768)*1.0/256;
	float A_y=(extreme_points[label*33+A_index*2+2]-32768)*1.0/256;

	float P_x=points[threadid*4+0]*1.0/256;
	float P_y=points[threadid*4+1]*1.0/256;

	float AB[2],AC[2],AP[2];
	AB[0]=B_x-A_x; //
	AB[1]=B_y-A_y;
	AC[0]=C_x-A_x;
	AC[1]=C_y-A_y;
	AP[0]=P_x-A_x;
	AP[1]=P_y-A_y;
	float dot_ABAB=dot_product(AB,AB);
	float dot_APAC=dot_product(AP,AC);
	float dot_APAB=dot_product(AP,AB);
	float dot_ACAC=dot_product(AC,AC);
	float dot_ABAC=dot_product(AB,AC);

	float u=(dot_ABAB*dot_APAC-dot_ABAC*dot_APAB)/(dot_ACAC*dot_ABAB-dot_ABAC*dot_ABAC);
	float v=(dot_ACAC*dot_APAB-dot_ABAC*dot_APAC)/(dot_ACAC*dot_ABAB-dot_ABAC*dot_ABAC);
	if (u>=0 && u<=1 && v>=0 && v<=1 && (u+v)<=1)
	{
		return;
	}
	if (u<0 && v>=0) //在AB的右侧
	{
		uint8_t new_area_label=area_label==1 ? 3 : 6;
		split_label[threadid]= new_area_label;
		float dis2line=fabs(dis2line_kernel(AB,AP));
		uint16_t dis2line_uint16=dis2line*256;
		all_dis2line[threadid]=dis2line_uint16;
		atomicMaxUint16_3(minmax_dis2line+label*14+new_area_label-1,dis2line_uint16);
	}
	else if (u>0 && v<=0) //在AC的左侧
	{ 
		uint8_t new_area_label=area_label==1 ? 4 : 5;
		split_label[threadid]= new_area_label;
		float dis2line=fabs(dis2line_kernel(AC,AP));
		uint16_t dis2line_uint16=dis2line*256;
		all_dis2line[threadid]=dis2line_uint16;
		atomicMaxUint16_3(minmax_dis2line+label*14+new_area_label-1,dis2line_uint16);
	}
}

//根据最远距离找到第2,6,10,14个极点
__global__ void get_extreme_points_stage2(const int16_t *points, int point_num,
	const uint8_t *points_label, uint16_t *extreme_points,uint8_t *split_label,
	uint16_t *all_dis2line,uint16_t *minmax_dis2line)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=(point_num))
	{
		return;
	}
	uint16_t label=points_label[threadid];
	if (label==0) return;

	if (extreme_points[label*33]==0) return;

	uint8_t area_label=split_label[threadid];
	if (area_label<=2) return;

	uint16_t extreme_dis=minmax_dis2line[label*14+area_label-1];
	uint16_t dis2line=all_dis2line[threadid];

	int extreme_index=area_label*4-10;
	if (extreme_dis==dis2line)
	{
		extreme_points[label*33+extreme_index*2+1]=points[threadid*4+0]+32768;
		extreme_points[label*33+extreme_index*2+2]=points[threadid*4+1]+32768;
	}
}

//根据找到的极点把点集分成三部分，三角形内，三角形左边，三角形右边，并且记录各自部分到三角形边的最远距离
//ABC是三角形的三个定点 P是任意一点，AP=u*AC+v*AB来表示, 1>=u>0 1>=v>0 u+v<=1表示点P在三角形ABC内
__global__ void split_points_stage3(const int16_t *points, int point_num,
	const uint8_t *points_label, const uint16_t *extreme_points,
	uint16_t *all_dis2line,uint16_t *minmax_dis2line, uint8_t *split_label)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=(point_num))
	{
		return;
	}
	uint16_t label=points_label[threadid];
	if (label==0) return;

	if (extreme_points[label*33]==0) return;

	uint8_t area_label=split_label[threadid];
	if (area_label<=2) return;
	int B_index=area_label*4-12;
	int C_index=(B_index+4)%16;


	float B_x=(extreme_points[label*33+B_index*2+1]-32768)*1.0/256; //最左端的点
	float B_y=(extreme_points[label*33+B_index*2+2]-32768)*1.0/256;
	float C_x=(extreme_points[label*33+C_index*2+1]-32768)*1.0/256; //最右端的点
	float C_y=(extreme_points[label*33+C_index*2+2]-32768)*1.0/256;

	int A_index=area_label*4-10;
	float A_x=(extreme_points[label*33+A_index*2+1]-32768)*1.0/256;
	float A_y=(extreme_points[label*33+A_index*2+2]-32768)*1.0/256;

	float P_x=points[threadid*4+0]*1.0/256;
	float P_y=points[threadid*4+1]*1.0/256;

	float AB[2],AC[2],AP[2];
	AB[0]=B_x-A_x; //
	AB[1]=B_y-A_y;
	AC[0]=C_x-A_x;
	AC[1]=C_y-A_y;
	AP[0]=P_x-A_x;
	AP[1]=P_y-A_y;
	float dot_ABAB=dot_product(AB,AB);
	float dot_APAC=dot_product(AP,AC);
	float dot_APAB=dot_product(AP,AB);
	float dot_ACAC=dot_product(AC,AC);
	float dot_ABAC=dot_product(AB,AC);

	float u=(dot_ABAB*dot_APAC-dot_ABAC*dot_APAB)/(dot_ACAC*dot_ABAB-dot_ABAC*dot_ABAC);
	float v=(dot_ACAC*dot_APAB-dot_ABAC*dot_APAC)/(dot_ACAC*dot_ABAB-dot_ABAC*dot_ABAC);
	if (u>=0 && u<=1 && v>=0 && v<=1 && (u+v)<=1)
	{
		return;
	}


	// if (P_y>0 && area_label==5)
	// {
	// 	// printf ("A: %d,%d\n",extreme_points[label*3+A_index*2+1],extreme_points[label*3+A_index*2+2]);
	// 	printf ("A: %f,%f,B: %f,%f,C:%f,%f,P:%f,%f,AB:%f,%f,AC: %f,%f,AP: %f,%f\n",
	// 		A_x,A_y,B_x,B_y,C_x,C_y,P_x,P_y,AB[0],AB[1],AC[0],AC[1],AP[0],AP[1]);
	// }
	// if (P_y>0 && area_label==5)
	// {
	// 	printf ("value %d: %d,%d,%d,%f,%f\n",label,A_index,B_index,C_index,u,v);
	// }

	if (u<0 && v>=0) //在AB的右侧
	{
		uint8_t new_area_label=area_label*2+1;
		split_label[threadid]= new_area_label;
		float dis2line=fabs(dis2line_kernel(AB,AP));
		uint16_t dis2line_uint16=dis2line*256;
		all_dis2line[threadid]=dis2line_uint16;
		atomicMaxUint16_3(minmax_dis2line+label*14+new_area_label-1,dis2line_uint16);
	}
	else if (u>0 && v<=0) //在AC的左侧
	{ 
		uint8_t new_area_label=area_label*2+2;
		split_label[threadid]= new_area_label;
		float dis2line=fabs(dis2line_kernel(AC,AP));
		uint16_t dis2line_uint16=dis2line*256;
		all_dis2line[threadid]=dis2line_uint16;
		atomicMaxUint16_3(minmax_dis2line+label*14+new_area_label-1,dis2line_uint16);
	}
}

//根据最远距离找到第2,6,10,14个极点
__global__ void get_extreme_points_stage3(const int16_t *points, int point_num,
	const uint8_t *points_label, uint16_t *extreme_points,uint8_t *split_label,
	uint16_t *all_dis2line,uint16_t *minmax_dis2line)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=(point_num))
	{
		return;
	}
	uint16_t label=points_label[threadid];
	if (label==0) return;

	if (extreme_points[label*33]==0) return;

	uint8_t area_label=split_label[threadid];
	if (area_label<=6) return;

	uint16_t extreme_dis=minmax_dis2line[label*14+area_label-1];
	uint16_t dis2line=all_dis2line[threadid];

	int extreme_index=area_label*2-13;
	// printf ("extreme_index %d %d: %d\n",label,area_label,extreme_index);
	if (extreme_dis==dis2line)
	{
		// printf ("extreme_index %d %d: %d\n",label,area_label,extreme_index);
		extreme_points[label*33+extreme_index*2+1]=points[threadid*4+0]+32768;
		extreme_points[label*33+extreme_index*2+2]=points[threadid*4+1]+32768;
	}
}

//凸外包的每条边都可以作为一个外接矩形的航向 通过所有凸外包的点来确认每个外包矩形的大小
__global__ void get_all_bbox(uint16_t *extreme_points,int label_num,float *all_bbox,
	float loc_x, float loc_y, float loc_yaw)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=(label_num*16*16))
	{
		return;
	}

	int label_id=threadid/(16*16);
	int sequence_id=(threadid%(16*16))%16;
	int point_id=(threadid%(16*16))/16;

	int start_warp_index=(point_id%2)*16;

	if (extreme_points[label_id*33]==0)
	{
		return;
	}

	uint16_t origin_x=extreme_points[label_id*33+point_id*2+1];
	uint16_t origin_y=extreme_points[label_id*33+point_id*2+2];
	if (origin_x==0 && origin_y==0)
	{
		return;
	}


	uint16_t yaw_start_x=extreme_points[label_id*33+sequence_id*2+1];
	uint16_t yaw_start_y=extreme_points[label_id*33+sequence_id*2+2];



	int count=1;
	uint16_t yaw_end_x=0,yaw_end_y=0;
	while (count<8)
	{
		int next=(sequence_id+count)%16;

		//需要同一个warp内的线程都跑到这一步 才能互相拿到值  如果有的线程break了 就不能拿到相应的值了
		uint16_t next_x=__shfl_sync(0xffffffff,yaw_start_x,start_warp_index+next);
		uint16_t next_y=__shfl_sync(0xffffffff,yaw_start_y,start_warp_index+next);
		// yaw_end_x=extreme_points[label_id*33+next*2+1];
		// yaw_end_y=extreme_points[label_id*33+next*2+2];
		// if (label_id==1 && point_id==2)
		// {
		// 	printf ("next point %d,%d: %d,%d,%d,%d,%d,%d\n",sequence_id,next,test_x,test_y,
		// 		yaw_end_x,yaw_end_y,start_warp_index,threadid);
		// }

		if (yaw_end_x==0 && yaw_end_y==0 && next_x!=0 && next_y!=0)
		{
			yaw_end_x=next_x;
			yaw_end_y=next_y;
		}
		count++;
	}

	if (yaw_start_x==0 && yaw_start_y==0)
	{
		return;
	}
	if (yaw_end_x==0 && yaw_end_y==0)
	{
		return;
	}

	float global_point[2]={(origin_x-32768)*1.0/256,(origin_y-32768)*1.0/256};
	transform2global(global_point,loc_x,loc_y,loc_yaw);
	float yaw_start_global[2]={(yaw_start_x-32768)*1.0/256,(yaw_start_y-32768)*1.0/256};
	transform2global(yaw_start_global,loc_x,loc_y,loc_yaw);
	float yaw_end_global[2]={(yaw_end_x-32768)*1.0/256,(yaw_end_y-32768)*1.0/256};
	transform2global(yaw_end_global,loc_x,loc_y,loc_yaw);


	float target_yaw=atan2f(yaw_end_global[1]-yaw_start_global[1],
		yaw_end_global[0]-yaw_start_global[0]);

	float new_x=global_point[0]*cos(target_yaw)+global_point[1]*sin(target_yaw);
	float new_y=-1*global_point[0]*sin(target_yaw)+global_point[1]*cos(target_yaw);

	int bbox_count=label_id*16*5+sequence_id*5;
	all_bbox[bbox_count]=target_yaw;
	fatomicMin2(all_bbox+bbox_count+1,new_x);
	fatomicMax2(all_bbox+bbox_count+2,new_x);
	fatomicMin2(all_bbox+bbox_count+3,new_y);
	fatomicMax2(all_bbox+bbox_count+4,new_y);
}

__global__ void get_all_bbox_cost(const int16_t* points, const uint8_t *points_label,
	int point_num,const float *all_bbox, int label_num,float *all_bbox_cost,
	float loc_x,float loc_y,float loc_yaw)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=(point_num*16))
	{
		return;
	}
	int sequence_id=threadid/point_num;
	int point_id=threadid%point_num;
	uint16_t label=points_label[point_id];
	if (label==0) return;

	int bbox_count=label*16*5+sequence_id*5;
	float target_yaw=all_bbox[bbox_count];
	if (target_yaw==0) return;


	float min_x=all_bbox[bbox_count+1];
	float max_x=all_bbox[bbox_count+2];
	float min_y=all_bbox[bbox_count+3];
	float max_y=all_bbox[bbox_count+4];

	float global_point[2]={points[point_id*4+0]*1.0/256,points[point_id*4+1]*1.0/256};
	transform2global(global_point,loc_x,loc_y,loc_yaw);

	float new_x=global_point[0]*cos(target_yaw)+global_point[1]*sin(target_yaw);
	float new_y=-1*global_point[0]*sin(target_yaw)+global_point[1]*cos(target_yaw);
	float dis_x=fmin(fabs(new_x-min_x),fabs(new_x-max_x));
	float dis_y=fmin(fabs(new_y-min_y),fabs(new_y-max_y));
	float dis=fmin(dis_x,dis_y);
	atomicAdd(all_bbox_cost+label*16+sequence_id,dis);


}

__global__ void get_min_bbox_cost(float *all_bbox,int label_num,float *all_bbox_cost,
	float *min_bbox_cost)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=(label_num*16))
	{
		return;
	}

	int label_id=threadid/16;
	int sequence_id=threadid%16;
	int bbox_count=label_id*16*5+sequence_id*5;
	if (all_bbox[bbox_count]==0) return;

	// if (label_id==1)
	// {
	// 	printf ("cost %d: %f\n",sequence_id,all_bbox_cost[threadid]);
	// }

	float cost=all_bbox_cost[threadid];
	fatomicMin2(min_bbox_cost+label_id,cost);
}

__global__ void get_final_bbox(float *all_bbox,int label_num,
	float *all_bbox_cost,float *min_bbox_cost,float *bbox)
{
	int threadid = blockDim.x * blockIdx.x + threadIdx.x;
	if (threadid>=(label_num*16))
	{
		return;
	}

	int label_id=threadid/16;
	int sequence_id=threadid%16;
	int bbox_count=label_id*16*5+sequence_id*5;
	if (all_bbox[bbox_count]==0) return;

	float cost=all_bbox_cost[threadid];
	if (cost==min_bbox_cost[label_id])
	{
		bbox[label_id*6+0]=2;
		bbox[label_id*6+1]=all_bbox[bbox_count];
		bbox[label_id*6+2]=all_bbox[bbox_count+1];
		bbox[label_id*6+3]=all_bbox[bbox_count+2];
		bbox[label_id*6+4]=all_bbox[bbox_count+3];
		bbox[label_id*6+5]=all_bbox[bbox_count+4];
	}
}


ConvexHull::ConvexHull(bool debug):
debug_(debug)
{
	if (debug_)
	{
		convexhull_display_=std::make_unique<ConvexHullDisplay>();
	}
	cuda_base::CUDA_CHECK_RETURN(cudaMalloc((void **)&extreme_points_gpu_,sizeof(uint16_t)*33*1000));
	cuda_base::CUDA_CHECK_RETURN(cudaMalloc((void **)&extreme_points_init_gpu_,sizeof(uint16_t)*33*1000));
	cuda_base::CUDA_CHECK_RETURN(cudaMalloc((void **)&split_label_gpu_,sizeof(uint8_t)*200000));
	cuda_base::CUDA_CHECK_RETURN(cudaMalloc((void **)&dis2line_gpu_,sizeof(uint16_t)*200000));
	cuda_base::CUDA_CHECK_RETURN(cudaMalloc((void **)&minmax_dis2line_gpu_,sizeof(uint16_t)*1000*14));
	cuda_base::CUDA_CHECK_RETURN(cudaMalloc((void **)&minmax_dis2line_init_gpu_,sizeof(uint16_t)*1000*14));
	cuda_base::CUDA_CHECK_RETURN(cudaMalloc((void **)&all_bbox_gpu_,sizeof(float)*1000*80));
	cuda_base::CUDA_CHECK_RETURN(cudaMalloc((void **)&all_bbox_init_gpu_,sizeof(float)*1000*80));
	cuda_base::CUDA_CHECK_RETURN(cudaMalloc((void **)&min_bbox_cost_gpu_,sizeof(float)*1000));
	cuda_base::CUDA_CHECK_RETURN(cudaMalloc((void **)&all_bbox_cost_gpu_,sizeof(float)*1000*16));
	cuda_base::CUDA_CHECK_RETURN(cudaMalloc((void **)&min_bbox_gpu_,sizeof(float)*1000*6));
	gen_init_gpu_value();
}

void ConvexHull::gen_init_gpu_value()
{
	uint16_t extreme_points[33*1000];
	for (int i=0;i<1000;i++)
	{
		for (int j=0;j<33;j++)
		{
			if (j==18)
			{
				extreme_points[i*33+j]=65535;
			}
			else
			{
				extreme_points[i*33+j]=0;
			}
		}
	}
	cuda_base::CUDA_CHECK_RETURN(cudaMemcpy(extreme_points_init_gpu_,extreme_points,
		sizeof(uint16_t)*33*1000,cudaMemcpyHostToDevice));

	uint16_t minmax_dis2line_init[1000*14];
	for (int i=0;i<1000;i++)
	{
		minmax_dis2line_init[i*14+0]=65535;
		for (int j=1;j<14;j++)
		{
			minmax_dis2line_init[i*14+j]=0;
		}
	}
	cuda_base::CUDA_CHECK_RETURN(cudaMemcpy(minmax_dis2line_init_gpu_,minmax_dis2line_init,
		sizeof(uint16_t)*14*1000,cudaMemcpyHostToDevice));

	float all_bbox[1000*80];
	for (int i=0;i<16000;i++)
	{
		all_bbox[i*5+0]=0;
		all_bbox[i*5+1]=1000000;
		all_bbox[i*5+2]=-1000000;
		all_bbox[i*5+3]=1000000;
		all_bbox[i*5+4]=-1000000;		
	}
	cuda_base::CUDA_CHECK_RETURN(cudaMemcpy(all_bbox_init_gpu_,all_bbox,
		sizeof(float)*1000*80,cudaMemcpyHostToDevice));
}

//先找到点集的最左点A和最右点B，根据AB连线把点集分成上（S1）下（S2)两部分，在S1中，找到距离
//直线AB最远的点C，ABC连成的三角形把S1分成三个部分，在三角形内，在AC外面S1.1和在BC外面S1.2，
//对S1.1和S1.2做找最远点的循环操作
//参考doc/convex_hull.jpg
//找到16个极点 形成凸包 会有极少点落在凸包外 忽略不计
void ConvexHull::get_convex_hull(const int16_t *points, int point_num,
	const uint8_t *points_label,int label_num,const module_base::Location &loc,
	std::vector<Eigen::MatrixXf> &objects_min_bbox)
{

	cuda_base::CUDA_CHECK_RETURN(cudaMemcpy(extreme_points_gpu_,extreme_points_init_gpu_,
		sizeof(uint16_t)*label_num*33,cudaMemcpyDeviceToDevice));
	int grid_num=(point_num+1024-1)/1024;
	get_leftright_points_y<<<grid_num,1024>>>(points,point_num,points_label,
		extreme_points_gpu_);
	get_leftright_points_x<<<grid_num,1024>>>(points,point_num,points_label,
		extreme_points_gpu_);

	cuda_base::CUDA_CHECK_RETURN(cudaMemset(split_label_gpu_,0,sizeof(uint8_t)*point_num));
	cuda_base::CUDA_CHECK_RETURN(cudaMemcpy(minmax_dis2line_gpu_,minmax_dis2line_init_gpu_,
		sizeof(uint16_t)*label_num*14,cudaMemcpyDeviceToDevice));
	split_points_stage1<<<grid_num,1024>>>(points, point_num,points_label,extreme_points_gpu_,
		dis2line_gpu_,minmax_dis2line_gpu_,split_label_gpu_);
	get_extreme_points_stage1<<<grid_num,1024>>>(points, point_num,points_label,extreme_points_gpu_,
		dis2line_gpu_,minmax_dis2line_gpu_);

	split_points_stage2<<<grid_num,1024>>>(points, point_num,points_label,extreme_points_gpu_,
		dis2line_gpu_,minmax_dis2line_gpu_,split_label_gpu_);
	get_extreme_points_stage2<<<grid_num,1024>>>(points, point_num,points_label,extreme_points_gpu_,
		split_label_gpu_,dis2line_gpu_,minmax_dis2line_gpu_);

	split_points_stage3<<<grid_num,1024>>>(points, point_num,points_label,extreme_points_gpu_,
		dis2line_gpu_,minmax_dis2line_gpu_,split_label_gpu_);
	get_extreme_points_stage3<<<grid_num,1024>>>(points, point_num,points_label,extreme_points_gpu_,
		split_label_gpu_,dis2line_gpu_,minmax_dis2line_gpu_);

	cuda_base::CUDA_CHECK_RETURN(cudaMemcpy(all_bbox_gpu_,all_bbox_init_gpu_,
		sizeof(float)*label_num*80,cudaMemcpyDeviceToDevice));
	cuda_base::CUDA_CHECK_RETURN(cudaMemset(min_bbox_cost_gpu_,100,sizeof(float)*label_num));
	cuda_base::CUDA_CHECK_RETURN(cudaMemset(all_bbox_cost_gpu_,0,sizeof(float)*label_num*16));


	int grid_num2=(label_num*16*16+1024-1)/1024;
	get_all_bbox<<<grid_num2,1024>>>(extreme_points_gpu_,label_num,all_bbox_gpu_,
		loc.x,loc.y,loc.yaw);

	// float *bbox_gpu_;
	// cudaMalloc((void **)&bbox_gpu_,sizeof(float)*label_num*5);
	int grid_num3=(point_num*16+1024-1)/1024;
	get_all_bbox_cost<<<grid_num3,1024>>>(points, points_label, point_num,
		all_bbox_gpu_,label_num,all_bbox_cost_gpu_,loc.x,loc.y,loc.yaw);

	cudaMemset(min_bbox_gpu_,0,sizeof(float)*label_num*6);
	int grid_num4=label_num*16;
	get_min_bbox_cost<<<grid_num4,1024>>>(all_bbox_gpu_,label_num,all_bbox_cost_gpu_,
		min_bbox_cost_gpu_);
	get_final_bbox<<<grid_num4,1024>>>(all_bbox_gpu_,label_num,all_bbox_cost_gpu_,
		min_bbox_cost_gpu_,min_bbox_gpu_);
	
	float *bbox_cpu=new float [label_num*6];
	cudaMemcpy(bbox_cpu,min_bbox_gpu_,sizeof(float)*label_num*6,
		cudaMemcpyDeviceToHost);

	for (int i=1;i<label_num;i++)
	{
		// printf ("label bbox debug %d: %f,%f,%f,%f,%f\n",i,bbox_cpu[i*6+1],
		// 	bbox_cpu[i*6+2],bbox_cpu[i*6+3],bbox_cpu[i*6+4],bbox_cpu[i*6+5]);
		float yaw=-1*bbox_cpu[i*6+1];
		// printf ("label %d: %d\n",i,all_bboxs.size());
		Eigen::MatrixXf single_bbox(2,4);
		single_bbox(0,0)=bbox_cpu[i*6+3];
		single_bbox(1,0)=bbox_cpu[i*6+5];
		single_bbox(0,1)=bbox_cpu[i*6+3];
		single_bbox(1,1)=bbox_cpu[i*6+4];
		single_bbox(0,2)=bbox_cpu[i*6+2];
		single_bbox(1,2)=bbox_cpu[i*6+4];
		single_bbox(0,3)=bbox_cpu[i*6+2];
		single_bbox(1,3)=bbox_cpu[i*6+5];
		Eigen::Matrix2f rotation;
		rotation << cos(yaw),sin(yaw),
				 -1*sin(yaw),cos(yaw);

		auto final_bbox=rotation*single_bbox;
		objects_min_bbox.push_back(final_bbox);
		// printf ("final_bbox sum: %f\n",final_bbox.sum());
	}


	if (debug_)
	{
		int16_t *debug_points=new int16_t [point_num*4];
		cudaMemcpy(debug_points,points,sizeof(int16_t)*point_num*4,
			cudaMemcpyDeviceToHost);
		uint16_t *extreme_points=new uint16_t [label_num*33];
		cudaMemcpy(extreme_points,extreme_points_gpu_,sizeof(uint16_t)*label_num*33,
			cudaMemcpyDeviceToHost);


		// for (int i=0;i<label_num;i++)
		// {
		// 	if (extreme_points[i*33]==0)
		// 	{
		// 		continue;
		// 	}
		// 	for (int j=1;j<16;j++)
		// 	{

		// 		if (extreme_points[i*33+j*2+1]!=0 || extreme_points[i*33+j*2+2]!=0)
		// 		{
		// 			continue;
		// 		}
		// 		int count=1;
		// 		for (int m=j+1;m<17;m++)
		// 		{
		// 			int next=m%16;
		// 			if (extreme_points[i*33+next*2+1]==0 || extreme_points[i*33+next*2+2]==0)
		// 			{
		// 				count++;
		// 				continue;
		// 			}

		// 			int diff_x=int(extreme_points[i*33+next*2+1])-int(extreme_points[i*33+(j-1)*2+1]);
		// 			int diff_y=int(extreme_points[i*33+next*2+2])-int(extreme_points[i*33+(j-1)*2+2]);
		// 			extreme_points[i*33+j*2+1]=extreme_points[i*33+(j-1)*2+1]+diff_x/(count+1);
		// 			extreme_points[i*33+j*2+2]=extreme_points[i*33+(j-1)*2+2]+diff_y/(count+1);
		// 			// printf ("value %d,%d: %d,%d,%d,%d,%d,%d\n",j-1,m,extreme_points[i*33+(j-1)*2+1],
		// 			// 	extreme_points[i*33+(j-1)*2+2],extreme_points[i*33+m*2+1],
		// 			// 	extreme_points[i*33+m*2+2],extreme_points[i*33+j*2+1],
		// 			// 	extreme_points[i*33+j*2+2]);
		// 			break;
		// 		}
		// 	}
		// }


		convexhull_display_->set_showinfo(debug_points,point_num);
		// convexhull_display_->set_showinfo(debug_points_float,point_num);
		convexhull_display_->set_extreme_points(extreme_points,label_num);
		convexhull_display_->set_bbox(bbox_cpu,label_num,loc);


		delete [] extreme_points;
		delete [] debug_points;
	}
	delete [] bbox_cpu;
}

ConvexHull::~ConvexHull()
{

}

}
